package com.sensePlusVR.setting.bluetooth;

import java.util.LinkedList;
import java.util.Queue;

import android.hardware.SensorManager;
import android.util.Log;

public class BTDataState {

	private static final String TAG = BTDataState.class.getSimpleName();
	private static final int CACHE_SIZE = 30;    //缓存的数据长度
	private static final double  coa_max = 0.95; //最大相关系数
	private static final double  coa_min = 0.8;  //最小相关系数
	private static final double  rang_min = 5.0f;
	private static final double  rang_max = 10.0f;
	private static final double  RANGE_LEVEL1 = 2.0f;
	private static final double  RANGE_LEVEL2 = 10.0f;
	private static final double  RANGE_LEVEL3 = 25.0f;
	private static final double  RANGE_LEVEL4 = 35.0f;
	
	private static final int IDLE_COUNT = 10;
	private static final int BUSY_COUNT = 5;
	private Queue<Double> mXData;  //x轴加速度
	private Queue<Double> mYData;  //y轴加速度
	private Queue<Double> mZData;  //z轴加速度
	private Queue<Double> mSData;  //实际加速度
	public static final int STATE_IDLE = 0;
	public static final int STATE_BUSY = 1;
	private int mBTState;
	private int mOldState;
	private double gravityX;
	private double gravityY;
	private double gravityZ;
	private final float alpha = 0.8f;
	private int bCount;
	private int iCount;
	
	private double x_max = 0.0f;
	private double x_min = 0.0f;
	
	
	public BTDataState(){
		mXData = new LinkedList<Double>();
		mYData = new LinkedList<Double>();
		mZData = new LinkedList<Double>();
		mSData = new LinkedList<Double>();
		mBTState = STATE_IDLE;
		bCount = 0;
		iCount = 0;
		mOldState = STATE_IDLE;
		
	}
	
	public int GetState(double x, double y, double z)
	{
		if (mXData.size() >= CACHE_SIZE)
		{
			mXData.poll();
			mYData.poll();
			mZData.poll();
			mSData.poll();
		}
				
		mXData.add(x);
		mYData.add(y);
		mZData.add(z);
		double s = Math.abs(Math.sqrt(x*x+y*y+z*z));
		mSData.add(s);		
		dataProc();

		if (mOldState == STATE_IDLE)
		{
			if (mBTState == STATE_BUSY)
			{
				bCount++;
			}
			if (mBTState == STATE_IDLE)
			{
				bCount = 0;
			}
			if (bCount > BUSY_COUNT)
			{
				mOldState = STATE_BUSY;
				bCount = 0;
			}
		}
		
		if (mOldState == STATE_BUSY)
		{
			if (mBTState == STATE_IDLE)
			{
				iCount++;
			}
			if (mBTState == STATE_BUSY)
			{
				iCount = 0;
			}
			if (iCount > IDLE_COUNT)
			{
				mOldState = STATE_IDLE;
				iCount = 0;
			}
			
		}		
		return mOldState;
		
	}
	
	
	private double GetRange(Queue<Double> q1)
	{
		double max = 0.0f;
		double min = 0.0f;
		double range = 0.0f;
		Object[] xArray = q1.toArray();
		for (int i = 0; i < xArray.length; i++)
		{
			double x = ((Double)xArray[i]).doubleValue();
			if (i == 0)
			{
				max = min = x;
			}
			if (max < x)
				max = x;
			if (min > x)
				min = x;
		}
		range = max - min;
		return range;
		
	}
	
	private void GetParam()
	{

		Object[] xArray = mXData.toArray();
		for (int i = 0; i < xArray.length; i++)
		{
			double x = ((Double)xArray[i]).doubleValue();
			if (i == 0)
			{
				x_max = x_min = x;
			}
			if (x_max < x)
				x_max = x;
			if (x_min > x)
				x_min = x;
		}
		
	}
	
	private double GetACF(Queue<Double> q1, Queue<Double> q2)
	{
		double sim = 0;
		double x_sum = 0.0f;
		double s_sum = 0.0f;
		double this_x_sum = 0.0f;
		double this_s_sum = 0.0f;
		double p_sum      = 0.0f;

		
		Object[] xArray = q1.toArray();
		Object[] sArray = q2.toArray();
		
		if (xArray.length != sArray.length)
		{
			Log.e(TAG, "Error! length should be equal!");
		}
		int length = xArray.length;
		for (int i = 0; i < length; i++)
		{
			double x = ((Double)xArray[i]).doubleValue();
			double s = ((Double)sArray[i]).doubleValue();

			x_sum += x;
			s_sum += s;
			this_x_sum += Math.pow(x, 2);
			this_s_sum += Math.pow(s, 2);
			p_sum += x*s;

		}
		if (length > 0)
		{
			double num = length*p_sum - x_sum*s_sum;
			double den = Math.sqrt((length * this_s_sum - Math.pow(s_sum, 2)) * (length * this_x_sum - Math.pow(x_sum, 2)));
			sim = (den == 0) ? 1 : num/den;		
		}
		return Math.abs(sim);
		
	}
	private void dataProc()
	{
		double acfSX = GetACF(mSData, mXData);
		double acfXZ = GetACF(mXData, mZData); 
		double x_range = GetRange(mXData);
		double y_range = GetRange(mYData);
		double z_range = GetRange(mZData);
		GetParam();

		

		if (x_range > RANGE_LEVEL3)
		{
			if (z_range > RANGE_LEVEL2)
			{
				if (acfSX > 0.6 && acfXZ > 0.6)
				{
					mBTState = STATE_BUSY;
				}
				else
				{
					mBTState = STATE_IDLE;
				}
			}
			else
			{
				if (acfSX > 0.8)
				{
					mBTState = STATE_BUSY;
				}
				else
				{
					mBTState = STATE_IDLE;
				}
			}
			
			

		}
		else if (x_range > RANGE_LEVEL1)
		{
			if (z_range > x_range)
			{
				if (acfSX >0.8 && acfXZ > 0.6)
				{
					mBTState = STATE_BUSY;
				}
				else
				{
					mBTState = STATE_IDLE;
				}
			}
			else if(z_range > x_range/2)
			{
				if (acfSX > 0.9 && acfXZ > 0.7)
				{
					mBTState = STATE_BUSY;
				}
				else
				{
					mBTState = STATE_IDLE;
				}
			}
			else
			{
				if (acfSX > 0.95)
				{
					mBTState = STATE_BUSY;
				}
				else
				{
					mBTState = STATE_IDLE;
				}
			}
//			if (acfSX > 0.9)
//			{
//				if (z_range > x_range/2 && acfXZ > 0.6)
//				{
//					mBTState = STATE_BUSY;
//				}
//				else if (z_range < x_range/2)
//				{
//					mBTState = STATE_BUSY;
//				}
//				else
//				{
//					mBTState = STATE_IDLE;
//				}
//			}
//			else if (z_range > RANGE_LEVEL3 && acfXZ > 0.8)
//			{
//				mBTState = STATE_BUSY;
//			}
//			else if (z_range > RANGE_LEVEL2 && y_range > RANGE_LEVEL2 && acfSX > 0.5 )
//			{
//				mBTState = STATE_BUSY;
//			}
//			else if (z_range > RANGE_LEVEL1 && acfXZ > 0.8 && acfSX > 0.8)
//			{
//				mBTState = STATE_BUSY;
//			}
//			else
//			{
//				mBTState = STATE_IDLE;
//			}	
			
		}
		else
		{
			mBTState = STATE_IDLE;
		}
		
		
		if (Math.abs(x_max) >= 19.5f || Math.abs(x_min) >= 19.5f)
		{
			mBTState = STATE_BUSY;
		}
//		else
//		{
//			if (acfSX > 0.9 && acfXZ > 0.7)
//			{
//				mBTState = STATE_BUSY;
//			}
//			else
//			{
//				mBTState = STATE_IDLE;
//			}
//		}
		
/**方案2**/
//		if (x_range > rang_min && acfSX > 0.9)
//		{
//			if (z_range > x_range/2 && acfXZ > 0.6)
//			{
//				mBTState = STATE_BUSY;
//			}
//			else if (z_range < x_range/2)
//			{
//				mBTState = STATE_BUSY;
//			}
//			else
//			{
//				mBTState = STATE_IDLE;
//			}
//		}
//		else if (z_range > rang_min && acfXZ > 0.9)
//		{
//			mBTState = STATE_BUSY;
//		}
//		else if (z_range > rang_mid && y_range > rang_mid && acfSX > 0.5 )
//		{
//			mBTState = STATE_BUSY;
//		}
//		else
//		{
//			mBTState = STATE_IDLE;
//		}	
/**方案1*/		
//		if (mBTState == STATE_BUSY )
//		{
//			if (acfSX < 0.95|| acfXZ < 0.6 || x_range < rang_min)
//				iCount++;
//			else
//				iCount = 0;
//		}
//		
//		if (bCount > BUSY_COUNT)
//		{
//			mBTState = STATE_BUSY;
//			bCount = 0;
//		}
//		
//		if (iCount > IDLE_COUNT)
//		{
//			mBTState = STATE_IDLE;
//			iCount = 0;
//		}
	}
}
